#include "mytftpserverif.h"
#include <string.h>
#include <stdio.h>
#include "main.h"
#include "flash_if.h"
#include "stdlib.h"
#include "string.h"

u32 iapbuf[512];
#define MM32_FLASH_BASE 0x08000000

__packed typedef struct {
    uint8_t   InitFlat;
    uint8_t   W_R_Mode;
    uint32_t  Flash_Addr ;
} Iap_FlashCont;


static  Iap_FlashCont   Iapflashcont ;



static void* OpenFile(const char* fname, const char* mode, u8_t write);
static void Close_File(void* handle);
static int Read_File(void* handle, void* buf, int bytes);
static int Write_File(void* handle, struct pbuf* p);

const struct tftp_context  tftpContext = {
    OpenFile,
    Close_File,
    Read_File,
    Write_File,
};

void MM32FLASH_Write(u32 WriteAddr, u32* pBuffer, u32 NumToWrite)
{
    u32 endaddr = 0;
    if(WriteAddr < MM32_FLASH_BASE || WriteAddr % 4)return;
    FLASH_Unlock();
    FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR);
    FLASH_ErasePage(WriteAddr);

    FLASH_ClearFlag(FLASH_FLAG_EOP );

    endaddr = WriteAddr + NumToWrite * 4;
    while(WriteAddr < endaddr) {
        if(FLASH_ProgramWord(WriteAddr, *pBuffer) != FLASH_COMPLETE) {
            break;
        }
        WriteAddr += 4;
        pBuffer++;
    }
    FLASH_ClearFlag(FLASH_FLAG_EOP );
    FLASH_Lock();
}

void iap_write_appbin(u32 appxaddr, u8* appbuf, u32 appsize)
{
    u32 t;
    u16 i = 0;
    u32 temp;
    u32 fwaddr = appxaddr;
    u8* dfu = appbuf;
    for(t = 0; t < appsize; t += 4) {
        temp = (u32)dfu[3] << 24;
        temp |= (u32)dfu[2] << 16;
        temp |= (u32)dfu[1] << 8;
        temp |= (u32)dfu[0];
        dfu += 4;
        iapbuf[i++] = temp;
        if(i == 256) {
            i = 0;
            MM32FLASH_Write(fwaddr, iapbuf, 256);
            fwaddr += 1024;
        }
    }
    if(i)MM32FLASH_Write(fwaddr, iapbuf, i);
}

////////////////////////////////////////////////////////////////////////////////
/// @brief  open file
/// @note   open  file.
/// @param  fname   :file handle.
///         mode  :write/read.
/// @retval >=0: Send successfully <0: Send failed.
////////////////////////////////////////////////////////////////////////////////
static void* OpenFile(const char* fname, const char* mode, u8_t write)
{
    printf("open file  %s \r\n", fname);
    printf("open mode  %s \r\n", mode);
    Iapflashcont.W_R_Mode = write ;
    Iapflashcont.W_R_Mode ? printf("write file\r\n") : printf("read file\r\n");

    if(Iapflashcont.W_R_Mode == 1) {
        FLASH_If_Init();
        Iapflashcont.Flash_Addr = USER_FLASH_FIRST_PAGE_ADDRESS ;
        printf("It takes a long time to erase Flash \r\n");
        if( FLASH_If_Erase(USER_FLASH_FIRST_PAGE_ADDRESS) == 0 ) {
            Iapflashcont.InitFlat = 1 ;
            printf("Write File Init Succes \r\n");
        }
    }
    else if(memcmp(fname, "flash.bin", strlen("flash.bin")) == 0) {
        Iapflashcont.InitFlat = 1 ;
        printf("Read File Init Succes \r\n");
        Iapflashcont.Flash_Addr = FLASH_BASE ;
    }
    return (Iapflashcont.InitFlat) ? (&Iapflashcont) : NULL ;
}


typedef  void (*iapfun)(void);
iapfun jump2app;

__asm void MSR_MSP(u32 addr)
{
    MSR MSP, r0             //set Main Stack value
    BX r14
}
////////////////////////////////////////////////////////////////////////////////
/// @brief  Close_File
/// @note   Close file.
/// @param  None.
/// @retval None.
////////////////////////////////////////////////////////////////////////////////
static void Close_File(void* handle)
{
    Iap_FlashCont* Filehandle = (Iap_FlashCont*) handle ;

    FLASH_If_UnInit();
    Filehandle->InitFlat = 0 ;
    printf("close file\r\n");
    if(Filehandle->W_R_Mode) {
        typedef  void (*pFunction)(void);
        /* Check if valid stack address (RAM address) then jump to user application */
        if (((*(__IO uint32_t*)USER_FLASH_FIRST_PAGE_ADDRESS) & 0x2FFE0000 ) == 0x20000000) {
            __set_PRIMASK(0) ;
            jump2app = (iapfun) * (vu32*)(USER_FLASH_FIRST_PAGE_ADDRESS + 4);
            MSR_MSP(*(vu32*)USER_FLASH_FIRST_PAGE_ADDRESS);
            jump2app();
            __set_PRIMASK(1) ;
        }
    }
}

////////////////////////////////////////////////////////////////////////////////
/// @brief  Read_File
/// @note   Read  File.
/// @param  handle   :file handle.
///         pbuf* p  :The data cache inside the data cache structure is all the actual
///                   data that needs to be written.
/// @retval >=0: Send successfully <0: Send failed.
////////////////////////////////////////////////////////////////////////////////
static int Read_File(void* handle, void* buf, int bytes)
{
    Iap_FlashCont* Filehandle = (Iap_FlashCont*) handle ;
    printf("Read file data and read length: %ld \r\n", bytes);
    if(!Filehandle->InitFlat) {
        return ERR_MEM ;
    }
    uint16_t Count ;
    for(  Count = 0 ; (Count < bytes) && (Filehandle->Flash_Addr <= FLASH_END)  ;  Count++, Filehandle->Flash_Addr++ ) {
        ((uint8_t*)buf)[Count] = *((__IO uint8_t*) Filehandle->Flash_Addr);
    }
    return Count;
}

////////////////////////////////////////////////////////////////////////////////
/// @brief  write file
/// @note   Close the socket.
/// @param  handle   :file handle.
///         pbuf* p  :The data cache inside the data cache structure is all the actual
///                   data that needs to be written.
/// @retval >=0: Send successfully <0: Send failed.
////////////////////////////////////////////////////////////////////////////////
static int Write_File(void* handle, struct pbuf* p)
{
    uint16_t Count ;
    Iap_FlashCont* Filehandle = (Iap_FlashCont*) handle ;
    printf("write file data length %ld \r\n", p->len);
    if(!Filehandle->InitFlat) {
        printf("write file is not initialized \r\n");
        return ERR_MEM ;
    }
    Count = p->len / 4 + ((p->len % 4) > 0);
    printf("Start writing FLASH address:0X%08X \r\n", Filehandle->Flash_Addr);
    if( FLASH_If_Write((uint32_t*)&Filehandle->Flash_Addr, (uint32_t*)p->payload, Count) == 0 ) {
        printf("The address of the next successful FLASH write:0X%08X \r\n", Filehandle->Flash_Addr);
        return ERR_OK;
    }
    else {
        printf("Error address when writing FLASH fails: 0X%08X \r\n", Filehandle->Flash_Addr);
    }
    return ERR_MEM;
}






